#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     SMUX_1,              sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     a,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     b,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     c,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     f,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     d,             tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    frontarm,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    backarm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    dispencer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Irpullup,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Irfront,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Irback,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/**********************************************************************\
*																																	    *
* PROGRAM: TeleOp program for Get Over It 2010-2011    	      			  *
* VERSION: 2.0  																									    *
* PURPOSE: Controllable user program for Tele-operational period.     *
Omni-bot must be configured according to the diagram on    *
the OMNI-driver.c file                                     *
* AUTHOR: Team R.A.B.B.I.                                             *
* DATE:		 April 2011  	  																					  *
*																																		  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/



/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef COMPLETEDRIVER
#include "drivers/CompleteDriver.h"
#endif

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
int speed;
void initializeRobot(){
  servo[frontarm] = 0;
  servo[backarm] = 244;
  servo[dispencer] = 5;
  HTSMUXinit();
  HTSMUXscanPorts(SMUX_1);
  HTSMUXscanPorts(SMUX_2);
  wait1Msec(50);
  speed = 100;    // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  StartTask(sensorRead);
  bDisplayDiagnostics = false;
  kalmindebugdisplay = true;
  eraseDisplay();
  startkalmin();
  wait1Msec(50);
  StartTask(Jdrive);
}



task main(){
  initializeRobot();
  speed = 100;
  waitForStart();   // wait for start of tele-op phase
  while (true){
    getJoystickSettings(joystick);
    if (joy1Btn(7) == 1 && joy1Btn(8) == 1){
      stopmot();
      resetkalmin();
      wait1Msec(4);}
    if (joy1Btn(7) ==1){
      speed=40;}
    if (joy1Btn(8) ==1){
      speed=20;}
    if (joy1Btn(7) ==0 && joy1Btn(8) ==0){
      speed=100;}
    if (joystick.joy1_TopHat == 0){      // If the topmost button on joy1's D-Pad ('TopHat') is pressed:
        forward(15);
    }
    if (joy1Btn(9) == 1){
      findgyrooffset();
    }
    else if (joystick.joy1_TopHat == 2){
      right(17);
    }
    else if (joystick.joy1_TopHat == 4){
      backward(17);
    }
    else if (joystick.joy1_TopHat == 6){
      left(17);
    }
    else {drive(speed);}
    if (joy1Btn(6) == 1){
      servo[frontarm] = 182;
    }
    else{
      servo[frontarm] = 0;
    }
    if (joy2Btn(4) == 1){
      allignHeight(RETRIEVING_HIGH);
    }
    else if (joy2Btn(3) == 1){
      allignHeight(RETRIEVING_MIDDLE);
    }
    else if (joy2Btn(1) == 1){
      allignHeight(DISPENCING_HEIGHT);
    }
    if (joy1Btn(5) == 1){
      servo[backarm] = 58;
    }
    else{
      servo[backarm] = 244;
    }
    motor[e] = -1 * (joystick.joy2_y2 / 6);
    motor[f] = -1 * (joystick.joy2_y1 / 6);
    wait1Msec(20);
  }
}
